// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SimpleCartesianMoveParameter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime.Arm
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Kinematics;

    using ShoNS.Array;

    /// <summary>
    /// A class that includes all the parameters for the $SimpleCartesianMoveAgent$ to work
    /// </summary>
    [DataContract]
    public class SimpleCartesianMoveParameter
    {
        /// <summary>
        /// A list of IK seeds, accessible through property $DefaultIKSeeds$
        /// </summary>
        private List<DoubleArray> defaultIKSeeds;

        /// <summary>
        /// The default IK seeds for serialization purpose only
        /// </summary>
        [DataMember(Name = "DefaultIKSeedsSerialized")]
        private List<double[]> defaultIKSeedsSerialized;

        /// <summary>
        /// Initializes a new instance of the <see cref="SimpleCartesianMoveParameter" /> class.
        /// </summary>
        /// <param name="kc">Arm kinematic chain</param>
        /// <param name="defaultIKSeeds">Default IK seeds</param>
        /// <param name="jointTolerance">Joint move tolerance</param>
        /// <param name="minimumJointHeight">Minimum height to avoid collision</param>
        /// <param name="maximumJointVel">Maximum joint velocity</param>
        /// <param name="cutOffPercentageForIKFiltering">The percentage of the top score for IK filtering</param>
        /// <param name="minimumTrajectoryTime">The minimum time for executing a trajectory</param>
        /// <param name="guaranteedClearance">The guaranteed clearance below the end effector</param>
        /// <param name="minimumElbowHeight">Minimum elbow height from table surface</param>
        /// <param name="trajectoryQualityThreshold">The threshold for trajectory quality</param>
        /// <param name="jointWeightFactors">Joint weight factors</param>
        /// <param name="elbowClearanceCheckList">Elbow clearance checklist</param>
        public SimpleCartesianMoveParameter(
            KinematicChain kc,
            List<double[]> defaultIKSeeds,
            double jointTolerance,
            double minimumJointHeight,
            double maximumJointVel,
            double cutOffPercentageForIKFiltering,
            double minimumTrajectoryTime,
            double guaranteedClearance,
            double minimumElbowHeight,
            double trajectoryQualityThreshold,
            double[] jointWeightFactors,
            bool[] elbowClearanceCheckList)
        {
            this.ArmKinematicChain = kc;
            this.defaultIKSeedsSerialized = defaultIKSeeds;
            this.JointMoveTolerance = jointTolerance;
            this.MinimumJointHeightFromTable = minimumJointHeight;
            this.MaximumJointVelocity = maximumJointVel;
            this.CutOffPercentageForIKSolutions = cutOffPercentageForIKFiltering;
            this.MinimumTrajectoryTime = minimumTrajectoryTime;
            this.GuaranteedSafeClearanceBelowEE = guaranteedClearance;
            this.MinimumElbowHeight = minimumElbowHeight;
            this.TrajectoryQualityThreshold = trajectoryQualityThreshold;
            this.JointWeightFactors = jointWeightFactors;
            this.ElbowClearanceCheckList = elbowClearanceCheckList;
        }

        /// <summary>
        /// Gets the maximum joint move tolerance in radians
        /// </summary>
        [DataMember]
        public double JointMoveTolerance { get; private set; }
        
        /// <summary>
        /// Gets the absolute minimum height a joint can be without colliding
        /// with the table
        /// </summary>
        [DataMember]
        public double MinimumJointHeightFromTable { get; private set; }

        /// <summary>
        /// Gets the maximum joint velocity
        /// </summary>
        [DataMember]
        public double MaximumJointVelocity { get; private set; }

        /// <summary>
        /// Gets the percentage of the top IK score based on which
        /// IK solutions are cut off
        /// </summary>
        [DataMember]
        public double CutOffPercentageForIKSolutions { get; private set; }

        /// <summary>
        /// Gets the absolute minimum time we want to spend for a trajectory in this agent
        /// </summary>
        [DataMember]
        public double MinimumTrajectoryTime { get; private set; }

        /// <summary>
        /// Gets the guaranteed clearance below palm, this should be assured by whoever uses this move agent.
        /// If this parameter is set non-zero and positive, it means that the space below the target
        /// Cartesian goal by this amount would be safe.
        /// </summary>
        [DataMember]
        public double GuaranteedSafeClearanceBelowEE { get; private set; }

        /// <summary>
        /// Gets the minimum elbow height
        /// </summary>
        [DataMember]
        public double MinimumElbowHeight { get; private set; }

        /// <summary>
        /// Gets the quality threshold for trajectory, which will be passed to a motion planner.
        /// This parameter is in experimental stage.
        /// </summary>
        [DataMember]
        public double TrajectoryQualityThreshold { get; private set; }

        /// <summary>
        /// Gets the joint weight factors
        /// </summary>
        [DataMember]
        public double[] JointWeightFactors { get; private set; }

        /// <summary>
        /// Gets the checklist for elbow height
        /// </summary>
        [DataMember]
        public bool[] ElbowClearanceCheckList { get; private set; }

        /// <summary>
        /// Gets the kinematic chain of the robot arm, used for IK computation
        /// </summary>
        [DataMember]
        public KinematicChain ArmKinematicChain { get; private set; }

        /// <summary>
        /// Gets the default IK seeds
        /// </summary>
        public List<DoubleArray> DefaultIKSeeds
        {
            get
            {
                if (null == this.defaultIKSeeds)
                {
                    this.defaultIKSeeds = new List<DoubleArray>();
                    foreach (double[] d in this.defaultIKSeedsSerialized)
                    {
                        this.defaultIKSeeds.Add(DoubleArray.From(d).Transpose());
                    }
                }

                return this.defaultIKSeeds;
            }

            private set
            {
                this.DefaultIKSeeds = value;
            }
        }
    }
}
